/*------------EE3032----group 1-4 -----------------------------------------*/
/*-------------slots map 1--3----------------------------------------------*/
/*-----------------------0--2----------------------------------------------*/
// all distances are in millimetre

#include "LPC17xx.h"
#include "lpc17xx_pwm.h"
#include "lpc17xx_uart.h"
#include "lpc17xx_pinsel.h"
#include "lpc17xx_gpio.h"
#include "lpc17xx_timer.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

// LCD define
#define LCD_Control_Port 0 // the port for LCD data pin (0 to 7)
#define LCD_RS 17
#define LCD_RW 15
#define LCD_E 16

// Stepper motor define
#define STEPPER_Port 0
#define STEPPER_dir 11
#define STEPPER_clock 10

// car park define
#define max_level 1
#define slots_per_level 3 // number of slots at each level
#define num_slots 3       // total number of slots
#define dist_per_level 80 // the height of each level (mm)
#define steps_per_lane 10 // number of step the stepper need to go for each lane

#define elevator_port 2	 // port of the elevator DC motor
#define elevator_pin 11  // pin of the elevator DC motor

// PID constants
#define Kp 40
#define Kpd 80
//#define Ki 100
//#define Kd 100
#define dist_threshold 10

/*------------------------------Constants-----------------------------------*/
const double Fs = 50.0; // PWM frequency is 50Hz
const double Fm = 0.5;  // frequency of modulating wave is 0.5Hz
const int M = 1;        // update duty every cycle
const int N = 100;  	// how many PWM cycles fit into one period of the modulating wave, taking into
                        // account the subsampling rate?


/*-------------------------------Global variable---------------------------*/
// global variables declaration
uint8_t current_level = 0;
uint8_t parking_slots[num_slots] = {0};
unsigned int last_height = 0;
unsigned char msg[16];

/*------------------------------Initialisation functions-------------------*/
// general GPIO pin initialisation
static void init_GPIO(int port, int pin, int dir){
	PINSEL_CFG_Type PinCfg;

	PinCfg.Funcnum = 0;

	PinCfg.OpenDrain = 0;
	PinCfg.Pinmode = 0;
	PinCfg.Portnum = port;
	PinCfg.Pinnum = pin;
	PINSEL_ConfigPin (&PinCfg);

	GPIO_SetDir (port, 1<<pin, dir);
}

// initialise the stepper motor pins
void STEPPER_init(){
	init_GPIO(STEPPER_Port, STEPPER_dir, 1);
	init_GPIO(STEPPER_Port, STEPPER_clock, 1);
}

void Elevator_motor_init(){
	// GPIO pin for enable pin in L298
	init_GPIO(elevator_port, elevator_pin, 1);
	// P2.3 PWM pin initialisation
}

void init_all_PWM(){
	// ensure PWM peripheral is powered on (it is powered on by default)
	LPC_SC->PCONP |= 1 << 6;

	LPC_PWM1->TCR = 2;                      // bring PWM module into reset
	LPC_PWM1->IR = 0xff;                    // clear any pending interrupts

	// configure P2.0 to P2.4 for PWM1.1 to PWM1.5 - O - Pulse Width Modulator 1-5, channel 1-5 output.
	LPC_PINCON->PINSEL4 = (LPC_PINCON->PINSEL4 & (~(0x3ff << 0))) | (0x155 << 0);

	// Disable pullups for P2.0 to P2.3
	LPC_PINCON->PINMODE4 = (LPC_PINCON->PINMODE4 & ~(0x3ff <<0)) | 0x2AA;
}

static void init_keypad(void) {
	// Initialise P0.0 to P0.3 as input
	uint8_t i;
	for (i = 0; i < 4; i++) {
		init_GPIO(0,i,1);
		GPIO_SetValue(0, 1 << i);
	}

	// Initialise P0.4 to P0.7
	for (i = 4; i < 8; i++)
		init_GPIO(0,i,0);

	//Initialise P2.10 as GPIO Interrupt
	PINSEL_CFG_Type PinCfg;
	PinCfg.Funcnum = 1;    //Function: EINT3
	PinCfg.OpenDrain = 0;
	PinCfg.Pinmode = 0;
	PinCfg.Portnum = 2;
	PinCfg.Pinnum = 10;
	PINSEL_ConfigPin(&PinCfg);
	//Enable EINT3 interrupt
}

int slot_servo(int8_t servonum, uint8_t dir) {
	//Set angle rotation of servo
	// Set prescale so we have a resolution of 1us
	//MR0: period of the pulse
	//MR1: pulse of PWM1.1
	//MR2: pulse of PWM1.2
	//MR3: pulse of PWM1.3
	//MR4: pulse of PWM1.4

	//servonum: decide what servo, 0-all servo intit; 1-4: servo; 5: elevator
	//dir: directory of movement: 0-anti clkwise, 1-clkwise

		LPC_PWM1->TCR = 2;
		LPC_PWM1->IR = 0xff;
		LPC_PWM1->PR = SystemCoreClock / (4 * 1000000) - 1;
		LPC_PWM1->MR0 = 20000;
	    if (servonum == -1) {
	    LPC_PWM1->MR1 = 2400;                   // set duty of 0.6ms
	    LPC_PWM1->MR2 = 2400;
	    LPC_PWM1->MR3 = 2400;
	    LPC_PWM1->MR4 = 2400;
	    }
	    else if (servonum == 0) {
	    	if (dir == 0) LPC_PWM1->MR1 = 600;
	    	else LPC_PWM1->MR1 = 2400;
	    }
	    else if (servonum == 1) {
	    	if (dir==0) LPC_PWM1->MR2 = 600;
			else LPC_PWM1->MR2 = 2400;
	    }
	    else if (servonum==2) {
	    	if (dir==0) LPC_PWM1->MR3 = 600;
	    	else LPC_PWM1->MR3 = 2400;
	    }
	    else if (servonum==3) {
	    	if (dir==0) LPC_PWM1->MR4 = 600;
	    	else LPC_PWM1->MR4 = 2400;
	    }

	    LPC_PWM1->LER = 0xf | (1 << 4);			// set latch-enable register
	    LPC_PWM1->MCR = 0x2;         				// reset on MR0
	    LPC_PWM1->PCR = (0xf << 9);				// enable PWM1.1 to PWM 1.4 with single-edge operation
	    LPC_PWM1->TCR = (1 << 3) | 1;           	// enable PWM mode and counting

	    return 0;
}



void all_servo_init(){
	//reset all servo, move all servo to -90 degree position
	slot_servo(-1,1);
}

void init_ADC() {
    //initialize the LED Pins as output
    LPC_GPIO1->FIODIR = 1 << 18 | 1 << 27;

    //Ensure the A/D converter is powered on
    LPC_SC->PCONP |= 1 << 12;

    //AD0.0 - I - A/D converter 0, input 0
    LPC_PINCON->PINSEL1 = (LPC_PINCON->PINSEL1 & ~(0x3 << 14)) | (0x1 << 14);

}


int elevator_PWM(uint8_t percent) {
	if (percent<50) LPC_PWM1->MR5 = percent;
	else if (percent>=50)  {
		LPC_PWM1->TCR = 2;
		LPC_PWM1->IR = 0xff;
		LPC_PWM1->PR = SystemCoreClock / (4 * 1000000) - 1;
		LPC_PWM1->MR0 = 100;
		LPC_PWM1->MR1 = 100;
		LPC_PWM1->MR2 = 100;
		LPC_PWM1->MR3 = 100;
		LPC_PWM1->MR4 = 100;
		LPC_PWM1->MR5 = percent;

		LPC_PWM1->LER = (0xf << 0) | (0x3 << 4);			// set latch-enable register
		LPC_PWM1->MCR = 0x2;         			// reset on MR0
		LPC_PWM1->PCR = (0xf << 9) | 1 << 13;				// enable PWM1.5 with single-edge operation
		LPC_PWM1->TCR = (1 << 3) | 1;           // enable PWM mode and counting
	}
	return 0;
}

void LCD_send_command(uint8_t ins){
	GPIO_SetValue(2, (ins%16) << 5);	// bits 0 to 3 are shifted 5
	GPIO_SetValue(0, (ins/16) << 6);	// bits 4 to 7 are shifted 6

	GPIO_ClearValue(2, ((ins%16) ^ 0xF) << 5);
	GPIO_ClearValue(0, ((ins/16) ^ 0xF) << 6);

	GPIO_ClearValue(LCD_Control_Port, 1 << LCD_RS);	// RS: Select command register
	GPIO_ClearValue(LCD_Control_Port, 1 << LCD_RW);	// RW: Write

	GPIO_SetValue(LCD_Control_Port, 1 << LCD_E);
	GPIO_ClearValue(LCD_Control_Port, 1 << LCD_E);	// Enable H->L

	Timer0_Wait(1);
}

void LCD_init(void){
	init_GPIO(2, 5, 1); //8-bit data
	init_GPIO(2, 6, 1);
	init_GPIO(2, 7, 1);
	init_GPIO(2, 8, 1);
	init_GPIO(0, 6, 1);
	init_GPIO(0, 7, 1);
	init_GPIO(0, 8, 1);
	init_GPIO(0, 9, 1);

	init_GPIO(LCD_Control_Port,LCD_E,1);
	init_GPIO(LCD_Control_Port,LCD_RW,1);
	init_GPIO(LCD_Control_Port,LCD_RS,1);

	Timer0_Wait(1);

	LCD_send_command(0x38);
	LCD_send_command(0x06);
	LCD_send_command(0x0E);
	LCD_send_command(0x01);
}

// Initialise EINT3 Interrupt
void EINT3_init (void){
	/*PINSEL_CFG_Type PinCfg;
	// P0.27
	PinCfg.Funcnum = 1;
	PinCfg.OpenDrain = 0;
	PinCfg.Pinmode = 0;
	PinCfg.Portnum = 0;
	PinCfg.Pinnum = 27;
	PINSEL_ConfigPin (&PinCfg);
	// P0.28
	PinCfg.Funcnum = 1;
	PinCfg.OpenDrain = 0;
	PinCfg.Pinmode = 0;
	PinCfg.Portnum = 0;
	PinCfg.Pinnum = 28;
	PINSEL_ConfigPin (&PinCfg);
	// P2.10
	PinCfg.Funcnum = 1;
	PinCfg.OpenDrain = 0;
	PinCfg.Pinmode = 0;
	PinCfg.Portnum = 2;
	PinCfg.Pinnum = 10;
	PINSEL_ConfigPin (&PinCfg);
	*/
	//Set the interrupt for rising edge
	LPC_GPIOINT->IO0IntEnR |= 1<<27;
	LPC_GPIOINT->IO0IntEnR |= 1<<28;
	//LPC_GPIOINT->IO2IntEnR |= 1<<10;

	//Enable EINT3 interrupt
	NVIC_EnableIRQ(EINT3_IRQn);
	//LPC_GPIOINT->IO2IntClr = 1 << 10;
}



void millis_init() {
	//static int _once = 0;

	LPC_SC->PCONP |= 1 << 23;     	//ensure timer3 is powered on
	LPC_TIM3->TCR = 2;				//put timer3 in reset

	//initialize variables
	//memset(_funcs, 0, sizeof(_funcs));
	//memset(_intervals, 0, sizeof(_intervals));

	LPC_TIM3->CTCR = 0;  			//timer mode
	LPC_TIM3->PR = 2400 -1;			//0.1ms resolution
	LPC_TIM3->MCR = 0;
	LPC_TIM3->CCR = 0;
	LPC_TIM3->IR = 0x3f;			//clear interrupt
	LPC_TIM3->TCR = 1;				//take out of reset and enable for counting

	// update for the LPC1769 dev board
	LPC_GPIO1->FIODIR = 1 <<29; 	//Initialize onboard LED
	LPC_GPIO1->FIOCLR = 1 <<29;
}

void system_init(){
	init_ADC();
	init_all_PWM();
	all_servo_init();
	init_keypad();
	STEPPER_init();
	Elevator_motor_init();
	LCD_init();
	EINT3_init();
	millis_init();
}

/*--------------------------------THIS IS THE ADC CODE--------------------------------*/
//Take sample from AD0.0 at 2Hz for 10 seconds and matching result with height
int elevator_to_ground() {
	// the function return the height of the elevator in mm, Lv1 is considered 0 mm
	// configure timer0 to generate a square wave on MAT0.1 ar rate of 2Hz
	int height;
	int counter;
	int upper_bound;
	int lower_bound;



	LPC_TIM0->TCR = 0x2;
	LPC_TIM0->CTCR = 0;

	//configure prescale register for resolution of 1 ms
	LPC_TIM0->PR = SystemCoreClock / (4*1000) - 1;

	LPC_TIM0->MCR = 1<10;				//reset on MR3
	LPC_TIM0->MR3 = 250;				//set 1/2 period of 250ms
	LPC_TIM0->EMR = 0x3 << 10;			//toogle MR3 on match

	NVIC_DisableIRQ(ADC_IRQn);

	LPC_ADC->ADCR = 0;


	//configure A/D converter for channel 0, clkdiv =2. power on
	LPC_ADC->ADCR = 1 | (2 << 8) |(1 << 21);
	//start conversion at the rising edge of MAT0.3 , the start bit must be assigned
	//inseperate steps or conversion will not start
	LPC_ADC->ADCR |= 0x5 <<24;

	//disable interrupt
	LPC_ADC->ADINTEN = 0;

	//select P0.23 for AD0.0 - I - A/D converter 0, input 0
	LPC_PINCON->PINSEL1 = (LPC_PINCON->PINSEL1 & ~(0x3 << 14)) | (0x1 << 14);

	//take out of reset and enable for counting
	LPC_TIM0->TCR = 1;

	//continuously check height and print feedback
	//wait for conversion to finish. bit 31 if the ADDR0 register will be
	//set when the conversion completes.
	while ((LPC_ADC->ADDR0 & (1 << 31)) == 0);
	//get the value. The value is between 1 and 4096
	int value = (LPC_ADC->ADDR0 >>4) & 0xfff;

	//printf("synchronous_timer(): %d\n\r", value);

	//matching value with height
	if ((value<3250) && (value>=2800)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 3250 - (counter-1)*45;
			lower_bound = 3250 - counter*45;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter-1;
				break;
			}
		}
	}
	else if ((value<2800) && (value>=2400)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 2800 - (counter-1)*40;
			lower_bound = 2800 - counter*40;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter+9;
				break;
			}
		}
	}
	else if ((value<2400) && (value>=2150)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 2400 - (counter-1)*25;
			lower_bound = 2400 - counter*25;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter+19;
				break;
			}
		}
	}
	else if ((value<2150) && (value>=1900)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 2150 - (counter-1)*25;
			lower_bound = 2150 - counter*25;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter+29;
				break;
			}
		}
	}
	else if ((value<1900) && (value>=1650)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 1900 - (counter-1)*25;
			lower_bound = 1900 - counter*25;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter+39;
				break;
			}
		}
	}
	else if ((value<1650) && (value>=1500)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 1650 - (counter-1)*15;
			lower_bound = 1650 - counter*15;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter+49;
				break;
			}
		}
	}
	else if ((value<1500) && (value>=1360)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 1500 - (counter-1)*14;
			lower_bound = 1500 - counter*14;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter+59;
				break;
			}
		}
	}
	else if ((value<1360) && (value>=1290)) {
		for (counter=1;counter<11;counter++) {
			upper_bound = 1360 - (counter-1)*7;
			lower_bound = 1360 - counter*7;
			if ((value<upper_bound) && (value>=lower_bound)){
				height= counter+69;
				break;
			}
		}
	}
	else if (value<1290) height = 80;
	else if (value>=3250) height = last_height;

	printf("Height: %d\n",height);
	last_height = height;
	return height;
}

void powerdown_ADC() {
	LPC_ADC->ADCR = 0;
	LPC_TIM0->TCR = 0;
}

/*-------------------------------THIS IS THE TIME COUNTER CODE----------------------------------------*/
//return the time since the processor start in millisecond
unsigned int millis(){
	return LPC_TIM3->TC/10;
}


//Time wait finction
void time_wait(int msec){
	unsigned int endtime = msec + millis();
	while (millis() < endtime);
}

/*-------------------------------THIS IS THE KEYPAD CODE----------------------------------------------*/
// Read the keypad
void keypad(){
	uint8_t i,j;
	uint8_t val;

    for (i = 0; i < 4; i++) {
		GPIO_ClearValue(0, 1<<i);

		val = (GPIO_ReadValue(0) >> 4) & 0x0f;

		if (val != 0xf) {
			if (i == 0) {
				if (val == 0xe) printf("Button 1\n");
				else if (val == 0xd) printf("Button 2\n");
				else if (val == 0xb) printf("Button 3\n");
				else if (val == 0x7) printf("Button 4\n");
			}
			else if (i == 1) {
				if (val==0xe) printf("Button 4\n");
				else if (val==0xd) printf("Button 5\n");
				else if (val==0xb) printf("Button 6\n");
				else if (val==0x7) printf("Button B\n");
			}
			else if (i == 2) {
				if (val==0xe) printf("Button 7\n");
				else if (val==0xd) printf("Button 8\n");
				else if (val==0xb) printf("Button 9\n");
				else if (val==0x7) printf("Button C\n");
			}
			else if (i == 3) {
				if (val == 0xe) printf("Button *\n");
				else if (val == 0xd) printf("Button 0\n");
				else if (val == 0xb) printf("Button #\n");
				else if (val == 0x7) printf("Button D\n");
			}

			Timer0_Wait(500);
		}
		for (j = 0; j < 4; j++)
			GPIO_SetValue(0, 1 << j);
    }
}

/*-------------------------------THIS IS SERVO CODE--------------------------------------*/


// drive the elevator DC motor
// speed from 0 to 50,
// dir: 0 = go up, 1 = go down
uint8_t Elevator_motor(uint8_t dir, uint8_t speed){
	if (speed > 50) speed = 50; // speed out of range

	if (speed == 0) {
		GPIO_ClearValue(elevator_port, 1 << elevator_pin);
	}
	else {
		// change the PWM duty cycle to change the motor speed
		if (dir == 0){
			if (elevator_PWM(50 - speed) == 1)
				return 1;
		}
		else {
			if (elevator_PWM(50 + speed) == 1)
				return 1;
		}
		GPIO_SetValue(elevator_port, 1 << elevator_pin);
	}
	return 0;
}

// drive the stepper motor
// dir = 1 is clockwise, 0 is anti-clockwise
void STEPPER_run(uint8_t dir, uint8_t steps){
	int i = 0;
	if (dir == 1)
		GPIO_SetValue(STEPPER_Port, 1<<STEPPER_dir);
	else
		GPIO_ClearValue(STEPPER_Port, 1<<STEPPER_dir);

	for(i=0; i<steps; i++){
		GPIO_ClearValue(STEPPER_Port, 1<<STEPPER_clock);
		//Timer0_Wait(1);
		GPIO_SetValue(STEPPER_Port, 1<<STEPPER_clock);
		//Timer0_Wait(1);
	}
}


void LCD_send_char(unsigned char var){
	GPIO_SetValue(2, (var%16) << 5);	// bits 0 to 3 are shifted 5
	GPIO_SetValue(0, (var/16) << 6);	// bits 4 to 7 are shifted 6

	GPIO_ClearValue(2, ((var%16) ^ 0xF) << 5);
	GPIO_ClearValue(0, ((var/16) ^ 0xF) << 6);

	GPIO_SetValue(LCD_Control_Port,1<<LCD_RS);		// RS: Select data register
	GPIO_ClearValue(LCD_Control_Port,1<<LCD_RW);	// RW: Write
	GPIO_SetValue(LCD_Control_Port,1<<LCD_E);
	GPIO_ClearValue(LCD_Control_Port,1<<LCD_E);		// Enable H->L

	Timer0_Wait(1);
}

void LCD_send_string(unsigned char *var){
	while(*var){
		LCD_send_char(*var++);		//send characters one by one
	}
}

// go to the given relative level
// level > 0 <=> go up
uint8_t goto_level(int8_t level){
	//unsigned int dist_sum = 0;
	//unsigned int last_dist = 0;
	int dist = 0;
	unsigned int speed = 0;
	uint8_t dir;

	if (level == 0 || abs(level) > max_level) return 0;

	// Enable the elevator DC motor PWM
	LPC_PWM1->TCR = 2;
	LPC_PWM1->IR = 0xff;
	LPC_PWM1->PR = SystemCoreClock / (4 * 1000000) - 1;
	LPC_PWM1->MR0 = 100;
	LPC_PWM1->MR1 = 100;
	LPC_PWM1->MR2 = 100;
	LPC_PWM1->MR3 = 100;
	LPC_PWM1->MR4 = 100;

	LPC_PWM1->LER = (0xf << 0) | (0x3 << 4);			// set latch-enable register
	LPC_PWM1->MCR = 0x2;         			// reset on MR0
	LPC_PWM1->PCR = (0xf << 9) | 1 << 13;				// enable PWM1.5 with single-edge operation
	LPC_PWM1->TCR = (1 << 3) | 1;           // enable PWM mode and counting

	current_level += level;
	do {
		// read the proximity sensor
		dist = elevator_to_ground() - (current_level*dist_per_level);
		if (dist < 0) dir = 0; // go up
		else dir = 1; // go  down
		dist = abs(dist);
		//dist_sum += dist;
		if (dir == 0)
			speed = Kp * dist / 100;
		else
			speed = Kpd * dist / 100;
		//+ Ki*dist_sum + Kd*(dist-last_dist);
		//last_dist = dist;

		// run the motor at speed v
		Elevator_motor(dir, speed);
	} while (dist > dist_threshold);

	Elevator_motor(dir, 0);
	return 0;
}

// go to the given relative lane
// negative = go to the lift side
// positive = go inside the car park
uint8_t goto_lane(int8_t lane){
	uint8_t dir;
	if (lane >= 0) dir = 0;
	else dir = 1;

	uint8_t steps = abs(lane) * steps_per_lane;
	// run stepper a steps number of steps
	STEPPER_run(dir, steps);

	return 0;
}

// put a car into an empty slot,
// take in the userID from the finger print reader
// return whether the car is successfully put into the slot
uint8_t car_in(uint8_t userID){
	int8_t level = 0;
	int8_t lane = 0;
	//uint8_t side = 0; //1 = left, 0 = right
	int empty_slot = 0;
	uint8_t result = 0;
	uint8_t i;

	for (i=0; i < num_slots; i++)
		if (parking_slots[i] == 0){
			empty_slot = i;								//find the nearest empty slot
			break;
		}

	level = (empty_slot / slots_per_level) + 1;
	lane  = (empty_slot % slots_per_level) / 2;
	// side  = (empty_slot % slots_per_level) % 2;

	// take the plate from the empty slot
	result = goto_level(level);
	if (result == 1){
		//error output
		return 1;
	}
	//time_wait(3000);

	result = slot_servo(0,0);
	if (result == 1){
			//error output
			return 1;
	}

	//time_wait(3000);

	result = goto_lane(lane);
	if (result == 1){
		//error output
		return 1;
	}
	//time_wait(3000);

	result = slot_servo(empty_slot,1);
	if (result == 1){
		//error output
		return 1;
	}

	//time_wait(3000);
	// put the car to the plate

	// put the plate back to the slot
	result = slot_servo(0,1);
	if (result == 1){
		//error output
		return 1;
	}
	result = goto_level(-level);
	if (result == 1){
		//error output
		return 1;
	}
	return 0;
}

uint8_t car_out(uint8_t target_slot){
	int8_t level = 0;
	int8_t lane = 0;
	uint8_t result = 0;

	level = (target_slot / slots_per_level) + 1;
	lane  = (target_slot % slots_per_level) / 2;

	result = goto_level(level);
	if (result == 1){
		//error output
		return 1;
	}

	time_wait(3000);

	result = goto_lane(lane);
	if (result == 1){
		//error output
		return 1;
	}

	time_wait(3000);

	result = slot_servo(target_slot,0);
	if (result == 1){
		//error output
		return 1;
	}

	time_wait(3000);

	result = goto_lane(-lane);
	if (result == 1){
		//error output
		return 1;
	}

	time_wait(3000);

	result = goto_level(-level);
	if (result == 1){
		//error output
		return 1;
	}

	return 0;
}



uint8_t process_car(ID){
	uint8_t carOut = 0;
	uint8_t i;

	for (i=1; i <= num_slots; i++)
		if (parking_slots[i] == ID)
			carOut = i;
	if (carOut == 0){
		LCD_send_command(0x01); //Clear screen, cursor home
		LCD_send_command(0x80); //Move cursor to first line
		LCD_send_string((unsigned char*)"DEPOSITING");
		car_in(ID);
	}
	else{
		LCD_send_command(0x01); //Clear screen, cursor home
		LCD_send_command(0x80); //Move cursor to first line
		LCD_send_string((unsigned char*)"RETRIEVING");
		car_out(carOut);
	}
	return 0;
}

//EINT3 Interrupt Handler
void EINT3_IRQHandler(){
	//Determine whether GPIO interrupt P2.10 has occurred
	uint8_t i;

	/*if ((LPC_GPIOINT->IO2IntStatR >> 10) & 0x1){
		// Smoke detected, emergency situation
		// Move the slots back
		for (i = 0; i < num_slots; i++)
			slot_servo(i, 1);
		// Move the elevator to base level
		goto_level(-current_level);
		// Output to the LCD
		LCD_send_command(0x01); //Clear screen, cursor home
		LCD_send_command(0x80); //Move cursor to first line
		//LCD_send_string((unsigned char*)"EMERGENCY");
		LCD_send_string((unsigned char*)"SMOKE DETECTED");
		//LCD_send_command(0xC0);	//Move cursor to second line

		//Clear GPIO Interrupt P2.10
		LPC_GPIOINT->IO2IntClr = 1 << 10;
		return;
	}*/
	// Determine whether GPIO interrupt P0.27 has occurred
	// Green button
	if ((LPC_GPIOINT->IO2IntStatR >> 27) & 0x1){
		process_car(1);
		//Clear GPIO Interrupt P2.10
		LPC_GPIOINT->IO0IntClr = 1 << 27;
	}

	// Determine whether GPIO interrupt P0.28 has occurred
	// Red Button
	if ((LPC_GPIOINT->IO0IntStatR >> 28) & 0x1){
		process_car(2);
		//Clear GPIO Interrupt P2.10
		LPC_GPIOINT->IO2IntClr = 1 << 28;
	}
}



int main(void){
	// Initialisation
	system_init();
	//parking_slots[0] = 5;

	LCD_send_command(0x01); //Clear screen, cursor home
	LCD_send_command(0x80); //Move cursor to first line
	LCD_send_string((unsigned char*)"WELCOME!!!");
	//process_car(1);
	//LPC_GPIOINT->IO2IntClr = 1 << 10;
	//goto_level(-1);
	// main loop
	while (1){
	}
	return 0 ;
}
